/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: mcl.h,v 1.2 2007-06-14 14:58:52 jari Exp $
 *
 */
#ifndef _MCL_H_
#define _MCL_H_

#define MCL_OK                  0
#define MCL_ERROR              -1
#include "mcl_interface.h"
#include "tpose.h"
#include <pthread.h>

/**
 *NOTE: These data types are now specified here but they SHOULD be defined in LnMlib Datatypes.
 *TODO: Well Fix it!
 */
namespace mcl{
    /**
     * Definition of scan for Scan Match interface
     * Uses the polar coordinate format (r,a) pairs. 
     * Natural for sensor coordinates and used as sensor coordinates here!
     */
    struct scan{
	public:
	    float *r;  ///< Distance measurement vector [m]
	    float *a;  ///< Angle measurement of distance measurement [rad] 
	    int N;     ///< The number of measurements in vector
	    scan(){
		N=0;
		r=NULL;
		a=NULL;
	    }
	    ~scan(){
		if(r) free(r);
		if(a) free(a);
	    }
	    void allocate(int n){
		r = (float *) malloc(n*sizeof(float));
		a = (float *) malloc(n*sizeof(float));
	    }
	    void copy(struct scan &new_scan){
		new_scan.r = (float *) malloc(N*sizeof(float));
		new_scan.a = (float *) malloc(N*sizeof(float));
		new_scan.N = N;
		memcpy(new_scan.r,r,N*sizeof(float));
		memcpy(new_scan.a,a,N*sizeof(float));
	    }
    };
	/**
     * ScanMatch pose definition
     * 2D-pose x,y and heading 
	 */
    struct pose{
	public:
	    float x; ///<[m]
	    float y; ///<[m]
	    float a; ///<[rad]
	    pose(){
		x=0;
		y=0;
		a=0;
	    }
	    void set(pose &p){
		x=p.x;
		y=p.y;
		a=p.a;
	    }
	    void set(float xx,float yy, float aa){
		x=xx;
		y=yy;
		a=aa;
	    }
	    ~pose(){}
		
    };
}



 typedef struct _mcl__
  {
    // Thread and state housekeeping
    pthread_t thread;
    pthread_mutex_t mutex;
    volatile int threadActive;
    
    ////// MCL Data //////
    TMCLData mcld;
  } TMCL;
  
  
  
  TMCL *mclCreate(void);
  int mclInitialize(TMCL *mcl);
  int mclDeinitialize(TMCL *mcl);
  void mclDestroy(TMCL *mcl);
  
  int mclStart(TMCL *mcl);
  int mclStop(TMCL *mcl);
  
  
  int mclGetPose(TMCL *mcl, TPose *pose);
  int mclSetPose(TMCL *mcl, const TPose *pose);
  int mclSetMap(TMCL *mcl, const char *mapdata, const int mapdatalen);
  

#endif //_MCL_H_
